\doxysection{C\+:/\+Users/\+ASUS/\+Desktop/dm-\/ctrl\+H7-\/balance-\/9025test/modules/algorithm/\+Quaternion\+EKF.c File Reference}
\hypertarget{_quaternion_e_k_f_8c}{}\label{_quaternion_e_k_f_8c}\index{C:/Users/ASUS/Desktop/dm-\/ctrlH7-\/balance-\/9025test/modules/algorithm/QuaternionEKF.c@{C:/Users/ASUS/Desktop/dm-\/ctrlH7-\/balance-\/9025test/modules/algorithm/QuaternionEKF.c}}


attitude update with gyro bias estimate and chi-\/square test  


{\ttfamily \#include "{}Quaternion\+EKF.\+h"{}}\newline
\doxysubsubsection*{Functions}
\begin{DoxyCompactItemize}
\item 
void \mbox{\hyperlink{_quaternion_e_k_f_8c_a43fed4fb5cef743a036ceb866de43628}{IMU\+\_\+\+Quaternion\+EKF\+\_\+\+Init}} (float \texorpdfstring{$\ast$}{*}init\+\_\+quaternion, float process\+\_\+noise1, float process\+\_\+noise2, float measure\+\_\+noise, float lambda, float lpf)
\begin{DoxyCompactList}\small\item\em Quaternion EKF initialization and some reference value. \end{DoxyCompactList}\item 
void \mbox{\hyperlink{_quaternion_e_k_f_8c_aae827fb4d03a3d115e6ad61501dda590}{IMU\+\_\+\+Quaternion\+EKF\+\_\+\+Update}} (float gx, float gy, float gz, float ax, float ay, float az, float dt)
\begin{DoxyCompactList}\small\item\em Quaternion EKF update. \end{DoxyCompactList}\end{DoxyCompactItemize}
\doxysubsubsection*{Variables}
\begin{DoxyCompactItemize}
\item 
\Hypertarget{_quaternion_e_k_f_8c_a2f6ee130c2912fa1106128c97aa54d69}\label{_quaternion_e_k_f_8c_a2f6ee130c2912fa1106128c97aa54d69} 
\mbox{\hyperlink{struct_q_e_k_f___i_n_s__t}{QEKF\+\_\+\+INS\+\_\+t}} {\bfseries QEKF\+\_\+\+INS}
\item 
const float \mbox{\hyperlink{_quaternion_e_k_f_8c_a5ff985346f2535a005af3c921af23f48}{IMU\+\_\+\+Quaternion\+EKF\+\_\+F}} \mbox{[}36\mbox{]}
\item 
float \mbox{\hyperlink{_quaternion_e_k_f_8c_a21d0ce3767bc89c03d0037a3a5bd219e}{IMU\+\_\+\+Quaternion\+EKF\+\_\+P}} \mbox{[}36\mbox{]}
\item 
\Hypertarget{_quaternion_e_k_f_8c_afee53610f74b2408ae7548f9cce19b54}\label{_quaternion_e_k_f_8c_afee53610f74b2408ae7548f9cce19b54} 
float {\bfseries IMU\+\_\+\+Quaternion\+EKF\+\_\+K} \mbox{[}18\mbox{]}
\item 
\Hypertarget{_quaternion_e_k_f_8c_a3011e2fa4795ab6a359d141fedef06ea}\label{_quaternion_e_k_f_8c_a3011e2fa4795ab6a359d141fedef06ea} 
float {\bfseries IMU\+\_\+\+Quaternion\+EKF\+\_\+H} \mbox{[}18\mbox{]}
\end{DoxyCompactItemize}


\doxysubsection{Detailed Description}
attitude update with gyro bias estimate and chi-\/square test 

\begin{DoxyAuthor}{Author}
Wang Hongxi 
\end{DoxyAuthor}
\begin{DoxyVersion}{Version}
V1.\+2.\+0 
\end{DoxyVersion}
\begin{DoxyDate}{Date}
2022/3/8 
\end{DoxyDate}
\begin{DoxyAttention}{Attention}
1st order LPF transfer function\+: 1 ——————— as + 1 
\end{DoxyAttention}


\label{doc-func-members}
\Hypertarget{_quaternion_e_k_f_8c_doc-func-members}
\doxysubsection{Function Documentation}
\Hypertarget{_quaternion_e_k_f_8c_a43fed4fb5cef743a036ceb866de43628}\index{QuaternionEKF.c@{QuaternionEKF.c}!IMU\_QuaternionEKF\_Init@{IMU\_QuaternionEKF\_Init}}
\index{IMU\_QuaternionEKF\_Init@{IMU\_QuaternionEKF\_Init}!QuaternionEKF.c@{QuaternionEKF.c}}
\doxysubsubsection{\texorpdfstring{IMU\_QuaternionEKF\_Init()}{IMU\_QuaternionEKF\_Init()}}
{\footnotesize\ttfamily \label{_quaternion_e_k_f_8c_a43fed4fb5cef743a036ceb866de43628} 
void IMU\+\_\+\+Quaternion\+EKF\+\_\+\+Init (\begin{DoxyParamCaption}\item[{float \texorpdfstring{$\ast$}{*}}]{init\+\_\+quaternion}{, }\item[{float}]{process\+\_\+noise1}{, }\item[{float}]{process\+\_\+noise2}{, }\item[{float}]{measure\+\_\+noise}{, }\item[{float}]{lambda}{, }\item[{float}]{lpf}{}\end{DoxyParamCaption})}



Quaternion EKF initialization and some reference value. 


\begin{DoxyParams}[1]{Parameters}
\mbox{\texttt{in}}  & {\em process\+\_\+noise1} & quaternion process noise 10 \\
\hline
\mbox{\texttt{in}}  & {\em process\+\_\+noise2} & gyro bias process noise 0.\+001 \\
\hline
\mbox{\texttt{in}}  & {\em measure\+\_\+noise} & accel measure noise 1000000 \\
\hline
\mbox{\texttt{in}}  & {\em lambda} & fading coefficient 0.\+9996 \\
\hline
\mbox{\texttt{in}}  & {\em lpf} & lowpass filter coefficient 0 \\
\hline
\end{DoxyParams}
\Hypertarget{_quaternion_e_k_f_8c_aae827fb4d03a3d115e6ad61501dda590}\index{QuaternionEKF.c@{QuaternionEKF.c}!IMU\_QuaternionEKF\_Update@{IMU\_QuaternionEKF\_Update}}
\index{IMU\_QuaternionEKF\_Update@{IMU\_QuaternionEKF\_Update}!QuaternionEKF.c@{QuaternionEKF.c}}
\doxysubsubsection{\texorpdfstring{IMU\_QuaternionEKF\_Update()}{IMU\_QuaternionEKF\_Update()}}
{\footnotesize\ttfamily \label{_quaternion_e_k_f_8c_aae827fb4d03a3d115e6ad61501dda590} 
void IMU\+\_\+\+Quaternion\+EKF\+\_\+\+Update (\begin{DoxyParamCaption}\item[{float}]{gx}{, }\item[{float}]{gy}{, }\item[{float}]{gz}{, }\item[{float}]{ax}{, }\item[{float}]{ay}{, }\item[{float}]{az}{, }\item[{float}]{dt}{}\end{DoxyParamCaption})}



Quaternion EKF update. 


\begin{DoxyParams}[1]{Parameters}
\mbox{\texttt{in}}  & {\em gyro} & x y z in rad/s \\
\hline
\mbox{\texttt{in}}  & {\em accel} & x y z in m/s² \\
\hline
\mbox{\texttt{in}}  & {\em update} & period in s \\
\hline
\end{DoxyParams}


\label{doc-var-members}
\Hypertarget{_quaternion_e_k_f_8c_doc-var-members}
\doxysubsection{Variable Documentation}
\Hypertarget{_quaternion_e_k_f_8c_a5ff985346f2535a005af3c921af23f48}\index{QuaternionEKF.c@{QuaternionEKF.c}!IMU\_QuaternionEKF\_F@{IMU\_QuaternionEKF\_F}}
\index{IMU\_QuaternionEKF\_F@{IMU\_QuaternionEKF\_F}!QuaternionEKF.c@{QuaternionEKF.c}}
\doxysubsubsection{\texorpdfstring{IMU\_QuaternionEKF\_F}{IMU\_QuaternionEKF\_F}}
{\footnotesize\ttfamily \label{_quaternion_e_k_f_8c_a5ff985346f2535a005af3c921af23f48} 
const float IMU\+\_\+\+Quaternion\+EKF\+\_\+F\mbox{[}36\mbox{]}}

{\bfseries Initial value\+:}
\begin{DoxyCode}{0}
\DoxyCodeLine{=\ \{1,\ 0,\ 0,\ 0,\ 0,\ 0,}
\DoxyCodeLine{\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 0,\ 1,\ 0,\ 0,\ 0,\ 0,}
\DoxyCodeLine{\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 0,\ 0,\ 1,\ 0,\ 0,\ 0,}
\DoxyCodeLine{\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 0,\ 0,\ 0,\ 1,\ 0,\ 0,}
\DoxyCodeLine{\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 0,\ 0,\ 0,\ 0,\ 1,\ 0,}
\DoxyCodeLine{\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 0,\ 0,\ 0,\ 0,\ 0,\ 1\}}

\end{DoxyCode}
\Hypertarget{_quaternion_e_k_f_8c_a21d0ce3767bc89c03d0037a3a5bd219e}\index{QuaternionEKF.c@{QuaternionEKF.c}!IMU\_QuaternionEKF\_P@{IMU\_QuaternionEKF\_P}}
\index{IMU\_QuaternionEKF\_P@{IMU\_QuaternionEKF\_P}!QuaternionEKF.c@{QuaternionEKF.c}}
\doxysubsubsection{\texorpdfstring{IMU\_QuaternionEKF\_P}{IMU\_QuaternionEKF\_P}}
{\footnotesize\ttfamily \label{_quaternion_e_k_f_8c_a21d0ce3767bc89c03d0037a3a5bd219e} 
float IMU\+\_\+\+Quaternion\+EKF\+\_\+P\mbox{[}36\mbox{]}}

{\bfseries Initial value\+:}
\begin{DoxyCode}{0}
\DoxyCodeLine{=\ \{100000,\ 0.1,\ 0.1,\ 0.1,\ 0.1,\ 0.1,}
\DoxyCodeLine{\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 0.1,\ 100000,\ 0.1,\ 0.1,\ 0.1,\ 0.1,}
\DoxyCodeLine{\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 0.1,\ 0.1,\ 100000,\ 0.1,\ 0.1,\ 0.1,}
\DoxyCodeLine{\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 0.1,\ 0.1,\ 0.1,\ 100000,\ 0.1,\ 0.1,}
\DoxyCodeLine{\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 0.1,\ 0.1,\ 0.1,\ 0.1,\ 100,\ 0.1,}
\DoxyCodeLine{\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 0.1,\ 0.1,\ 0.1,\ 0.1,\ 0.1,\ 100\}}

\end{DoxyCode}
